/** @file actor.c
    @brief Implementations of definitions in actor.h

    @author Juan Pablo Carbajal
    
    @date 21. July, 2008
    

*/
#define PRIME_SEED 27644437
#define COU_MKS  8.987551787e9
#define MAG_MKS 1e-7
#include "actor.h"
#include <map>

//Locales
static Sing *aux; 
static Sing *aux2;

//Implementations
void vibMotor (BehavData* data)
{
	static cpVect noise,app=cpvzero;
	static cpFloat theta;
	
	noise=cpv(2.0e0*rand()/((double)RAND_MAX)-1.0e0,2.0e0*rand()/((double)RAND_MAX)-1.0e0);
	theta=(rand()/((double)RAND_MAX))*6.28;
	app=cpv(cos(theta),sin(theta));
	cpBodyApplyForce( data->body,cpvmult(noise,data->param[0]),cpvmult(app,data->param[1]) );
}

void FillForceData(Sing* source,int inds, Sing* obs,int indo, ForceData* data)
{
	
	// Global Positon and orientation of the source singularity
	 data->p0=source->Gpos[inds];
	 data->ang0=source->Gangle[inds]; 
	 
	// Global Position and orientation of the observed singularity
	 data->p=obs->Gpos[indo];
	 data->ang=obs->Gangle[indo];
	
	// Derived magnitudes
	 data->relp=cpvsub(data->p,data->p0); //Relative position
	 data->r[0]=cpvlength(data->relp); // Distance
	 data->r[1]=cpvlengthsq(data->relp); // Square Distance
	 data->r[2]=data->r[0]*data->r[1]; // Cubic distance
	 
     source->force_func[inds](data); // The value of the force
	 data->F= cpvmult(data->F,source->sing[inds].value*obs->sing[indo].value);
}

void CoulombForce(ForceData* data){
	data->F=cpvmult(cpvnormalize(data->relp),COU_MKS/data->r[1]);
}

void PerpMagDipoleForce(ForceData* data){
	data->F=cpvmult(cpvnormalize(data->relp),3.e0*MAG_MKS/(data->r[1]*data->r[1]));
}

void MagDipoleForce(ForceData* data){
	static cpFloat phi,alpha,beta,Fr,Fphi;
	
	// Angle of the relative position vector
//	phi=cpvtoangle(data->relp);
	phi=cpvtoangle(data->relp);
//	if( (phi=cpvtoangle(data->relp)) > M_PI)
//		phi-=2.0e0*M_PI;
	alpha=data->ang0;
//	if( (alpha=data->ang0) > M_PI)
//		alpha-=2.0e0*M_PI;
	beta=data->ang;		
//	if( (beta=data->ang) > M_PI)
//		beta-=2.0e0*M_PI;

	alpha =phi - alpha;
	beta = phi - beta;
	
	
	// Components in polar coordinates
	Fr=(2.0e0*cos(alpha)*cos(beta) - sin(alpha)*sin(beta));
	Fphi=sin(alpha+beta);
	
	// Cartesian coordinates
	data->F=cpv(Fr*cos(phi)-Fphi*sin(phi),Fr*sin(phi)+Fphi*cos(phi));
	data->F=cpvmult(data->F,-3.e0*MAG_MKS/(data->r[1]*data->r[1]));
}
void MagDipoleTorque(ForceData* data){
	static cpFloat phi,alpha,beta;
	
	// Angle of the relative position vector
	phi=cpvtoangle(data->relp);
//	if( (phi=cpvtoangle(data->relp)) > M_PI)
//		phi-=2.0e0*M_PI;
	alpha=data->ang0;
//	if( (alpha=data->ang0) > M_PI)
//		alpha-=2.0e0*M_PI;
	beta=data->ang;		
//	if( (beta=data->ang) > M_PI)
//		beta-=2.0e0*M_PI;
//	printf("p=%g a=%g b=%g\n",phi,alpha/M_PI,beta/M_PI);
	alpha =phi - alpha;
	beta = phi - beta;
	// Torque
	data->F.x=(MAG_MKS/data->r[2])*(3.0e0*cos(alpha)*sin(beta) + sin(alpha-beta));
}

void LRangeForceApply(cpBody *a, cpBody *b){
	
	aux = (Sing*)((ActorData*)a->data)->singularity;
	aux2 = (Sing*)((ActorData*)b->data)->singularity;
	cpVect delta;
	// General data needed to calculate interaction
	static ForceData fdata;
	fdata.F=cpvzero;
	// Calculate the forces between the charges of different bodies
	for (int i=0; i<aux->Nsing; i++)
	{
		for (int j=0; j<aux2->Nsing; j++)
		{
			/** @todo There should be an index of interacting singularities */
			if(!strcmp(aux->sing[i].type,aux2->sing[j].type))
			{
				FillForceData (aux2,j,aux,i,&fdata);
				
				//Force applied to body A
				delta=cpvsub(fdata.p,a->p);
				cpBodyApplyForce(a,fdata.F,delta);
				
				//Reaction on body B
				delta=cpvsub(fdata.p0,b->p);
				cpBodyApplyForce(b,cpvneg(fdata.F),delta);
				
				if(aux->torque_func[i] != NULL)
				{
					//Torque on A
					aux->torque_func[i](&fdata);
					a->t+=aux->sing[i].value*aux2->sing[j].value*fdata.F.x;
					
					//Torque on B
					fdata.ang0=aux->Gangle[i];
					fdata.ang=aux2->Gangle[j];
					fdata.relp=cpvneg(fdata.relp);
					aux2->torque_func[j](&fdata);
					b->t+=aux->sing[i].value*aux2->sing[j].value*fdata.F.x;
				}
			}
		}
	}
}

void ChargedBodyUpdatePosition(cpBody *body, cpFloat dt)
{
	/** @warning This function is outdated. Contains bugs. */
	cpVect dp = cpvmult(cpvadd(body->v, body->v_bias), dt);
	body->p = cpvadd(body->p, dp);

	cpBodySetAngle(body, body->a + (body->w + body->w_bias)*dt);

	// Update position of the charges
	aux = (Sing*)((ActorData*)body->data)->singularity;
	for (int i=0; i<aux->Nsing; i++)
            aux->Gpos[i]=cpvadd(body->p,cpvrotate(cpv(aux->sing[i].position.x,
													  aux->sing[i].position.y), body->rot));
            
 	body->v_bias = cpvzero;
	body->w_bias = 0.0f;
}
void ChargedBodyUpdateVelocity(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt)
{
	/** @warning This function is outdated. Contains bugs. */
	body->v = cpvadd(cpvmult(body->v, damping), cpvmult(cpvadd(gravity, cpvmult(body->f, body->m_inv)), dt));
	body->w = body->w*damping + body->t*body->i_inv*dt;
}

void ChargedBodyUpdatePositionVerlet(cpBody *body, cpFloat dt)
{
	cpVect dp = cpvmult(cpvadd(body->v, body->v_bias), dt);
	dp = cpvadd(dp,cpvmult(cpvmult(body->f,body->m_inv),0.5e0*dt*dt));
	body->p = cpvadd(body->p, dp);

	cpBodySetAngle(body, body->a + (body->w + body->w_bias)*dt 
				   + 0.5*body->t*body->i_inv*dt*dt);

	// Update position of the singularities
	aux = (Sing*)((ActorData*)body->data)->singularity;
	for (int i=0; i<aux->Nsing; i++)
	{
        aux->Gpos[i]=cpvadd(body->p,cpvrotate(cpv(aux->sing[i].position.x,
										  aux->sing[i].position.y), body->rot));
		aux->Gangle[i]= aux->sing[i].angle + body->a;
	}
	
            
 	body->v_bias = cpvzero;
	body->w_bias = 0.0f;
}

void ChargedBodyUpdateVelocityVerlet(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt)
{
	body->v = cpvadd(body->v, cpvmult(cpvadd(gravity, cpvmult(body->f, body->m_inv)), 0.5e0*dt));
	body->w = body->w + body->t*body->i_inv*0.5e0*dt;
	
	body->f=cpvzero;
	body->t=0.0e0;
	
	// Long range interaction
	/** @todo The Verlet integration needs to update the forces. Because it is inside
	a loop over all the bodies we cannot use directly LongRangeForceapply. At the moment the
	update is done manually.*/
	
	static cpArray *bodies = space->bodies;
	static cpBody* B;
	static cpVect delta;
	aux=(Sing*)((ActorData*)body->data)->singularity;

	// General data needed to calculate interaction
	static ForceData fdata;
	fdata.F=cpvzero;
	
	for(int i=0; i< bodies->num; i++)
	{
	  B=(cpBody*)bodies->arr[i];
	  aux2=(Sing*)((ActorData*)B->data)->singularity;
	  if(B != body)
	  {
        // Calculate the forces between the singularities of different bodies
        for (int j=0; j<aux->Nsing; j++)
	    {
         for (int k=0; k<aux2->Nsing; k++)
         {
			if(!strcmp(aux->sing[j].type,aux2->sing[k].type))
		   {
			   FillForceData (aux2,k,aux,j,&fdata);

			   //Force applied to body 
			   delta=cpvsub(fdata.p,body->p);
			   cpBodyApplyForce(body,fdata.F,delta);
			   
			   //Torque on A
			   if(aux->torque_func[j] != NULL)
			   {
			    aux->torque_func[j](&fdata);
			    body->t+=aux->sing[j].value*aux2->sing[k].value*fdata.F.x;
			   }
		   }
		 }
	    }
	  }
	}
	body->v = cpvadd(cpvmult(body->v,damping), cpvmult(cpvadd(gravity, cpvmult(body->f, body->m_inv)), 0.5e0*dt));
	body->w = body->w*damping + body->t*body->i_inv*0.5e0*dt;
}

void ReadKinFile(const char* kinfile, int bodiesnum, 
				 cpFloat* ang, cpFloat* w, cpVect* p, cpVect* v)
{
	FILE* data;
	
	// Open file
	if( (data = fopen(kinfile, "r"))==NULL)
	{
		perror(kinfile);
		exit (1);
	}
	// Read the values
	char c,line[1000];
	int num;
	
	
	while((c=fgetc(data)) !=EOF)
	{
    	if(c=='#')
		{
			fscanf(data,"%s",line);
			printf("\n Reading %s....",line);
			if(!strcmp(line,"Position"))
			{
				for(int i=0; i<num; i++)
					fscanf(data,"%lf %lf",&(p[i].x),&(p[i].y));
			}
			else if (!strcmp(line,"Velocity"))
			{
				for(int i=0; i<num; i++)
					fscanf(data,"%lf %lf",&(v[i].x),&(v[i].y));
			}
			else if (!strcmp(line,"Rotation"))
			{
				for(int i=0; i<num; i++)
					fscanf(data,"%lf",&ang[i]);

			}
			else if (!strcmp(line,"AngularVelocity"))
			{
				for(int i=0; i<num; i++)
					fscanf(data,"%lf",&w[i]);

			}
			else if (!strcmp(line,"NumberOfActors"))
			{
				fscanf(data,"%d",&num);
				printf("\t\n Number of actors = %d",num);

				if(num!=bodiesnum)
				{
					perror("Non matching number of actors");
					exit(1);
				}
			}	
		}
	}
	fclose(data);
}
void WriteKinFile(FILE* data, int num, cpFloat* ang,
							cpFloat* w, cpVect* p, cpVect* v)
{
	static int Nmandatory=5;
	static char* mandatoryfields[100]={
		"NumberOfActors",
		"Position",
		"Velocity",
		"Rotation",
		"AngularVelocity"
	};
	static int fieldTyp[5]={0,2,2,1,1};
	static char* fmt[10]={
		"%d\n", // 0 = integer
		"%16.10e\n", //1 = real
		"%16.10e %16.10e\n" //2 = real vector
	};
	
	// Preamble
	static char* prea="Kinematic file for SA2D. The word following the number symbol will activate the\nparsing.\nThe number of actors is mandatory.";
	fprintf(data,"%s\nFile created at iteration = %d\n",prea,ticks);

	for(int i=0; i<Nmandatory; i++)
	{
		fprintf(data,"# %s\n",mandatoryfields[i]);
		switch(i)// Can this be improved?
		{
			case 0:
				fprintf(data,fmt[fieldTyp[i]],num);
				break;
			case 1:
				for(int j=0; j<num; j++)
					fprintf(data,fmt[fieldTyp[i]],p[j].x,p[j].y);
				break;
			case 2:
				for(int j=0; j<num; j++)
					fprintf(data,fmt[fieldTyp[i]],v[j].x,v[j].y);
				break;
			case 3:
				for(int j=0; j<num; j++)
					fprintf(data,fmt[fieldTyp[i]],ang[j]);
				break;
			case 4:
				for(int j=0; j<num; j++)
					fprintf(data,fmt[fieldTyp[i]],w[j]);
				break;
		}
				
	}
		
}
void convertKinematic(paramSet* kin, acKinematic* pAcKinematic){

	pAcKinematic->source = kin->getSource().c_str();
	
	pAcKinematic->Nparam = kin->getParam_qty();
	
	pAcKinematic->params = (acParam*)malloc((pAcKinematic->Nparam)*sizeof(acParam));

	map<string, string> params = kin->getParams();

	map<string, string>::iterator it;

	int i = 0;
	  for ( it=params.begin() ; it != params.end(); it++ ){
		  cout << "here! Nparam Kin=" << pAcKinematic->Nparam << endl;
		  pAcKinematic->params[i].name =(*it).first.c_str();
		  pAcKinematic->params[i].value = (*it).second.c_str();
		  i++;
		  	
	  }
	  pAcKinematic->Nparam = i;
		
}

void LoadActorKin(agentSet* set,int nset)
{
	acKinematic kin ;
	
	convertKinematic(set->getInitialCondition(), &kin);
	
	cpFloat *ang,*w;
	cpVect *p,*v;
	cpArray* bodies=space->bodies;

	ang=(cpFloat*)malloc(set->getElem_quantity()*sizeof(cpFloat));
	w=(cpFloat*)malloc(set->getElem_quantity()*sizeof(cpFloat));
	p=(cpVect*)malloc(set->getElem_quantity()*sizeof(cpVect));
	v=(cpVect*)malloc(set->getElem_quantity()*sizeof(cpVect));
	
	for(int i=0; i<set->getElem_quantity(); i++)
	{
		ang[i]=0.0e0;
		w[i]=0.0e0;
		p[i]=cpvzero;
		v[i]=cpvzero;
	}
	
	if(!strcmp(kin.source,"file"))
	{
		/** Look for the parameters */
		for(int i=0; i<kin.Nparam; i++)
		{
			if(!strcmp(kin.params[i].name,"filename"))
			{
				printf("Reading kinematics from %s ....",kin.params[i].value);
				// Read the data
				ReadKinFile(kin.params[i].value,set->getElem_quantity(),
							ang, w, p, v);
				printf("OK\n");
			}
		}
	}
	else if(!strcmp(kin.source,"random"))
	{
		char msg[255]="";
		sprintf(msg,"Reading %s Kinematics:",kin.source);
		printf("%s ",msg);

		static int ncall=0;
		srand(PRIME_SEED+ncall);
		ncall+=2;
		/** Look for the parameters */
		for(int i=0; i<kin.Nparam; i++)
		{
			printf("%s \t",kin.params[i].name);
			if(!strcmp(kin.params[i].name,"pos"))
			{
				for(int j=0; j<set->getElem_quantity(); j++)
				{
					cpFloat r1=2.0e0*rand()/((double)RAND_MAX)-1.0e0;
					cpFloat r2=2.0e0*rand()/((double)RAND_MAX)-1.0e0;
					p[j]=cpvmult(cpv(r1,r2),atof(kin.params[i].value));
					cout << "x: " << p[j].x << endl;
				}
			}
			else if(!strcmp(kin.params[i].name,"vel"))
			{
				for(int j=0; j<set->getElem_quantity(); j++)
				{
					cpFloat r1=2.0e0*rand()/((double)RAND_MAX)-1.0e0;
					cpFloat r2=2.0e0*rand()/((double)RAND_MAX)-1.0e0;
					v[j]=cpvmult(cpv(r1,r2),atof(kin.params[i].value));
				}
			}
			else if(!strcmp(kin.params[i].name,"ang"))
			{
				for(int j=0; j<set->getElem_quantity(); j++)
				{
					cpFloat r1=2.0e0*rand()/((double)RAND_MAX)-1.0e0;
					ang[j]=r1*atof(kin.params[i].value);
				}
			}
			else if(!strcmp(kin.params[i].name,"angvel"))
			{
				for(int j=0; j<set->getElem_quantity(); j++)
				{
					cpFloat r1=2.0e0*rand()/((double)RAND_MAX)-1.0e0;
					w[j]=r1*atof(kin.params[i].value);
				}

			}
		}
		printf("\n");
	}

	// Asign the values to the actors
	for(int i=0; i< set->getElem_quantity(); i++)
	{
		int k=actorIndex[nset][i];
		cpBody* actor=(cpBody*)bodies->arr[k];
		actor->p=p[i]; //Position
		actor->v=v[i]; //Speed
		
		// Rotation angle
		cpBodySetAngle(actor, ang[i]);
		// Rotation speed
		actor->w=w[i];
	}
	free(ang);free(w);free(v);free(p);
}

void initActors(configurationfile* objectmodel)
{	
	cpShape* ashape;
	cpBody* actor;
	cpVect* aux3;
	std::vector<agentSet*> agents = objectmodel->getAgentSets();
	//static cpJoint* joint;
	
	// Prepare the index of the actors
	int actor_n=0;
	actorIndex=(int**)malloc(agents.size()*sizeof(int));


	std::vector<agentSet*>::iterator it;
	geometry *currentGeometry;
	std::vector<vertex*> vertices;

	for (unsigned int i = 0;i<agents.size();i++){
		actorIndex[i]=(int*)malloc(agents[i]-> getElem_quantity()*sizeof(int));
	}
	// Loop over the sets
	for (unsigned int i = 0;i<agents.size();i++) //it=agents.begin(); it<agents.end(); it++
	{
		//Loop over the elements (actors) of the set[i]
		for (int j=0; j<agents[i]-> getElem_quantity(); j++)
		{
			currentGeometry = agents[i]->getGeometry();
			vertices = currentGeometry->getVertices();

			/********** Center of Mass *************/
			cpVect CoM;
			CoM=cpv(-agents[i]->getPhysicalProperties()->getCenterOfMass()->getX(),
					-agents[i]->getPhysicalProperties()->getCenterOfMass()->getY());
			/** @warning When a scale is defined the units will change.*/

			if(currentGeometry->getShape_type().compare("convpoly")==0)
			{
				/********** Vertices *************/
				aux3=(cpVect*)malloc(vertices.size()*sizeof(cpVect));
				
				for(unsigned int k=0; k<vertices.size();k++){
					aux3[k]=cpv(vertices[k]->getX(), vertices[k]->getY());
				}

				/********** Mass and Moment of inertia *************/
				actor = cpBodyNew(agents[i]->getPhysicalProperties()->getMass(),
								  cpMomentForPoly(agents[i]->getPhysicalProperties()->getMass(),
												  vertices.size(),
												  aux3, CoM));
				
				cpSpaceAddBody(space, actor);
				actorIndex[i][j]=actor_n;
				actor_n+=1;
				/********** Shape (Collision detection) *************/
				ashape = cpPolyShapeNew(actor, vertices.size(),
									   aux3, CoM);

				ashape->e = agents[i]->getPhysicalProperties()->getElasticity();
				printf("%g \n",ashape->e);
				ashape->u =  agents[i]->getPhysicalProperties()->getFriction();
				/** @todo Surface speed for the actors */
				
				cpSpaceAddShape(space, ashape);
				
				free(aux3);
			}// End convpoly actors
			else if(currentGeometry->getShape_type().compare("circle")==0)
			{

				/********** Mass and Moment of inertia *************/
				actor = cpBodyNew(agents[i]->getPhysicalProperties()->getMass(),
								  cpMomentForCircle(agents[i]->getPhysicalProperties()->getMass(),0.0,
										  (cpFloat)((circularGeometry*) currentGeometry)->getRadius(),CoM));
				
				
				cpSpaceAddBody(space, actor);
				actorIndex[i][j]=actor_n;
				actor_n+=1;
				/********** Shape (Collision detection) *************/
				ashape = cpCircleShapeNew(actor,  (cpFloat)((circularGeometry*) currentGeometry)->getRadius(), CoM);

				ashape->e = agents[i]->getPhysicalProperties()->getElasticity();
				ashape->u =  agents[i]->getPhysicalProperties()->getFriction();

				/** @todo Surface speed for the actors */
				
				cpSpaceAddShape(space, ashape);
			}//End Circle actor
			else
			{
				perror("Actor shape type unrecognized");
				exit(1);
			}

		}//End Loop over actors	
		
		/********** Kinematics *************/
		printf("Set: %d %s\n",i,agents[i]->getName().c_str());
		LoadActorKin(agents [i],i);
		/** @warning if set name in xml is repeated then segmentation fault */
		
		cpArray* bodies=space->bodies;
		
		/********** The void pointer DATA *************/
		ActorData * ac;
		for (int j=0; j<agents[i]->getElem_quantity(); j++)
		{
			ac=(ActorData*)malloc(sizeof(ActorData));
			
			/***Information***/

			//@todo andri, cast is no a nice solution
			ac->setname = (char*)agents[i]->getName().c_str();

			cpBody *body = (cpBody *)bodies->arr[actorIndex[i][j]];
			body->data=ac;

			/*** Singularities ***/
			aux=(Sing*)malloc(sizeof(Sing));
			vector<singularity*> singularities = agents[i]->getSingularities();
			aux->Nsing=singularities.size();
			aux->sing=(acSingularity*)malloc(aux->Nsing*sizeof(acSingularity));
			aux->Gpos=(cpVect*)malloc(aux->Nsing*sizeof(cpVect));
			/** @todo The pointer Gangle could be smaller because is used to store
			 information for singularities of the type magdipole only. Using C++ 
			 containers would simplify this. */
			aux->Gangle=(cpFloat*)malloc(aux->Nsing*sizeof(cpFloat));
			/** @todo By defining the number of different types of singularities
			 this vector can be small. Indeed of the size of the number of type 
			 of singularities. Using ENUM would be a possibility */
			aux->force_func=(SingForceFunc*)malloc(aux->Nsing*sizeof(SingForceFunc));
			aux->torque_func=(SingForceFunc*)malloc(aux->Nsing*sizeof(SingForceFunc));
			
			for(int k=0;k<aux->Nsing;k++)
			{

				aux->sing[k].angle = singularities[k]->getAngle()->getRadian();
				aux->sing[k].type = (char*)singularities[k]->getType().c_str();
				aux->sing[k].value = singularities[k]->getValue();
				aux->sing[k].position.x = singularities[k]->getPosition()->getX();
				aux->sing[k].position.y = singularities[k]->getPosition()->getY();

				aux->Gpos[k]=cpvadd(body->p,
									cpv(aux->sing[k].position.x,
										aux->sing[k].position.y));
				aux->Gangle[k]=0.0e0;
				if(!strcmp(aux->sing[k].type,"electrical"))
				{
					aux->force_func[k]=CoulombForce;
					aux->torque_func[k]=NULL;
				}
				else if (!strcmp(aux->sing[k].type,"magdipole_perp"))
				{
					aux->force_func[k]=PerpMagDipoleForce;
					aux->torque_func[k]=NULL;
				}
				else if (!strcmp(aux->sing[k].type,"magdipole"))
				{
					aux->Gangle[k]=body->a + aux->sing[k].angle;
					aux->force_func[k]=MagDipoleForce;
					aux->torque_func[k]=MagDipoleTorque;
				}
				else
				{
					perror("Singularity type unrecognized");
					exit(1);
				}
			}
			((ActorData*)body->data)->singularity=aux;
			
			/*** Behaviors ***/
			Behav* behavaux;
			behavaux=(Behav*)malloc(sizeof(Behav));
			/** @todo Add behaviors simultaneusly */
			behavaux->Nbehav=1;
			behavaux->behav_func=(BehaviorFunc*)malloc(behavaux->Nbehav*sizeof(BehaviorFunc));
			//printf("%s\n",param.sets[1].behavior.params[0].value);
			/** @todo Define the function setBehavoirFunc(behavaux->behav_func); */
			behavaux->behav_func[0]=vibMotor;

			//Andri new way of dfining behavior data
//			vector<paramSet*> behaviors = agents[i]->getArtificialIntelligence();
//
//			for (int i=0; i < behaviors.size(); i++) {
//				behavaux->data.params[i]=
//
//				bdata.param[0]=atof(((ActorData*)bdata.body->data)->behavior->data.params[0].value);
//			}


			((ActorData*)body->data)->behavior=behavaux;
			

//		  body->position_func=ChargedBodyUpdatePosition;
		  body->position_func=ChargedBodyUpdatePositionVerlet;		//General properties that apply for both types
//		  body->velocity_func=ChargedBodyUpdateVelocity;
		  body->velocity_func=ChargedBodyUpdateVelocityVerlet;
	   }
	}//End Loop over Sets
	if(actor_n!=space->bodies->num)
	{
		perror("Error indexing actors");
		exit(1);
	}
/** @todo Program the joints! */
/*	for (int i=1; i<param.Nact; i++){
		aux3=cpvadd(actor[i-1]->p,param.vertex[3]);
		joint=cpPivotJointNew(actor[i-1],actor[i],cpvzero);
	        cpSpaceAddJoint(space, joint);
	}*/
}
